#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include "rosmsg/route.h"
#include "rosmsg/gps.h"
#include "control.h"


geometry_msgs::Twist twist;
rosmsg::route route;


void callBack(const geometry_msgs::Twist::ConstPtr& msg)
{
	twist = *msg;
}


void routeCallBack(const rosmsg::route::ConstPtr& msg)
{
	route = *msg;
	printf("======= route array: %lu =======\r\n", route.lon.size());

	for (int i = 0; i < route.lon.size(); i++)
	{
		printf("lon: %.9lf \tlat: %.9lf\r\n", route.lon[i], route.lat[i]);
	}
}



void system_init()
{
	char item[4][128];
	char port_name[4][128];

	int get_config(char item[4][128], char port_name[4][128]);

	int num = get_config(item, port_name);

	for (int i = 0; i < num; ++i)
	{
		if (strcmp(item[i], "base") == 0)
		{
			base_init(port_name[i]);
		}

		if (strcmp(item[i], "imu") == 0)
		{
			imu_init(port_name[i]);
		}

		if (strcmp(item[i], "gps") == 0)
		{
			gps_init(port_name[i]);
		}

		if (strcmp(item[i], "radar") == 0)
		{
			radar_init(port_name[i]);
		}
	}
}


void remote_key_control(void)
{
	if (fabs(twist.angular.z) > 1.0)
	{
		int rspeed = (int)twist.angular.z * 20.0;
		rotation_speed(rspeed);
		twist.angular.z = 0.0;
	}

	if (fabs(twist.linear.x) > 1.0)
	{
		int lspeed = (int)twist.linear.x * 20.0;
		linear_speed(lspeed);
		twist.linear.x = 0.0;
	}
}


int main(int argc, char** argv)
{
	rosmsg::gps ros_gps;
	struct gps cur_gps;

	ros::init(argc, argv, "tank_exe");
	ros::NodeHandle nh;

	ros::Subscriber subKey = nh.subscribe("/tank/cmd_vel", 100, callBack);
	ros::Subscriber subRoute = nh.subscribe("/route", 100, routeCallBack);
	ros::Publisher pubGPS = nh.advertise<rosmsg::gps>("/gps", 100);

	// system_init();

	while (ros::ok())
	{
		remote_key_control();
		// tank_control(37, 103, 27, 103);

		if (gps_update() == 2 && get_gps(&cur_gps) == 1)
		{
			if (route.lon.size() > 0) //必须接收到地图下发的路径才进行坦克控制和计算目标距离和方向
			{
				if(tank_control(cur_gps.latitude, cur_gps.longitude, route.lat[0], route.lon[0]))
				{
					route.lon.erase(route.lon.begin());
					route.lat.erase(route.lat.begin());
				}

			    printf("clan:%.9lf clon:%.9lf dlan:%.9lf dlon:%.9lf\n", cur_gps.latitude, cur_gps.longitude, route.lat[0], route.lon[0]);
			}


			ros_gps.lat = cur_gps.latitude;
			ros_gps.lon = cur_gps.longitude;
			pubGPS.publish(ros_gps);
		}

		ros::spinOnce();
	}

	return 0;
}
